#!/usr/bin/python3
from launch import LaunchDescription
from launch_ros.actions import Node, LifecycleNode
from launch.actions import IncludeLaunchDescription, TimerAction
import xacro
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
import os

def generate_launch_description():
    # 获取雷达参数文件路径
    lslidar_dir = get_package_share_directory('lslidar_driver')
    lidar_param_file = os.path.join(lslidar_dir, 'params', 'lidar_uart_ros2', 'lsn10.yaml')
    
    # 获取机器人描述文件
    robot_desc_dir = get_package_share_directory('chaojikeaidexiaoche_description')
    xacro_file = os.path.join(robot_desc_dir, 'urdf', 'chaojikeaidexiaoche.xacro')
    
    # 处理xacro文件
    robot_description_config = xacro.process_file(xacro_file)
    robot_description = {'robot_description': robot_description_config.toxml()}
    
    # 获取cartographer配置文件路径
    cartographer_dir = get_package_share_directory('fruit_cartographer')
    cartographer_launch_file = os.path.join(cartographer_dir, 'launch', 'cartographer_localization.launch.py')
    
    # 获取双相机启动文件路径
    fruit_cam_dir = get_package_share_directory('fruit_cam')
    dual_camera_launch_file = os.path.join(fruit_cam_dir, 'launch', 'dual_camera.launch.py')
    
    # 获取MoveIt demo启动文件路径
    moveit_dir = get_package_share_directory('chaojixiaoche_moveit')
    demo_launch_file = os.path.join(moveit_dir, 'launch', 'demo.launch.py')
    
    # 获取水果识别模型路径
    fruit_detector_dir = get_package_share_directory('fruit_detector')
    model_path_a = os.path.join(fruit_detector_dir, 'model', 'A.onnx')
    model_path_b = os.path.join(fruit_detector_dir, 'model', 'B.onnx')
    model_path_c = os.path.join(fruit_detector_dir, 'model', 'C.onnx')
    
    # 加载MoveIt配置（用于arm_control）
    moveit_config = MoveItConfigsBuilder("chaojikeaidexiaoche", package_name="chaojixiaoche_moveit").to_moveit_configs()
    
    
    # N10串口雷达节点
    lidar_node = LifecycleNode(
        package='lslidar_driver',
        executable='lslidar_driver_node',
        name='lslidar_driver_node',
        output='screen',
        emulate_tty=True,
        namespace='',
        parameters=[lidar_param_file]
    )
    
    
    
    # 导航节点
    fruit_nav_node = Node(
        package='fruit_nav',
        executable='fruit_nav_node',
        name='fruit_nav',
        output='screen'
    )
    
    # 引入cartographer定位启动文件（延迟1秒启动）
    cartographer_localization_include = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(cartographer_launch_file),
        launch_arguments={
            'use_sim_time': 'false',
            'start_rviz': 'true'  # 启动cartographer的RViz可视化
        }.items()
    )
    
    # 使用TimerAction延迟4秒启动cartographer（等待MoveIt先启动）
    cartographer_localization = TimerAction(
        period=4.0,  # 延迟4秒
        actions=[cartographer_localization_include]
    )
    
    # 使用TimerAction延迟6秒启动导航节点（等待Cartographer先启动）
    delayed_nav_node = TimerAction(
        period=6.0,  # 延迟6秒
        actions=[fruit_nav_node]
    )
    
    # 引入MoveIt demo启动文件
    demo_include = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(demo_launch_file)
    )
    
    # 使用TimerAction延迟2秒启动MoveIt demo（等待雷达先启动）
    delayed_demo = TimerAction(
        period=2.0,
        actions=[demo_include]
    )
    
    # arm_control action服务器节点
    arm_control_node = Node(
        package='arm_control',
        executable='arm_control_node',
        name='arm_control_server',
        output='screen',
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
            moveit_config.planning_pipelines,
        ],
    )
    
    # 轨迹发布节点
    trajectory_publisher_node = Node(
        package='arm_control',
        executable='trajectory_publisher_node',
        name='trajectory_publisher',
        output='screen',
        parameters=[
            {'publish_interval_ms': 100},  # 每150ms发布一个轨迹点（增加间隔让下位机有更多处理时间）
            {'point_skip_interval': 4}     # 每2个点发布一个（减少转发点数量）
        ],
    )
    
    # 使用TimerAction延迟5秒启动arm_control相关节点
    delayed_arm_control = TimerAction(
        period=5.0,  # 延迟5秒
        actions=[arm_control_node, trajectory_publisher_node]
    )
    
    # 引入双相机启动文件
    dual_camera_include = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(dual_camera_launch_file)
    )
    
    # 使用TimerAction延迟7秒启动双相机（等待arm_control先启动）
    delayed_dual_camera = TimerAction(
        period=7.0,  # 延迟7秒，等待arm_control先启动
        actions=[dual_camera_include]
    )
    
    # 二维码识别节点
    qr_recognition_node = Node(
        package='qr_recognition',
        executable='qr_recognition_server.py',
        name='qr_recognition_server',
        output='screen'
    )
    
    # 使用TimerAction延迟10秒启动二维码识别（等待相机先启动）
    delayed_qr_recognition = TimerAction(
        period=10.0,  # 延迟10秒，等待相机先启动
        actions=[qr_recognition_node]
    )
    
    # 水果识别节点
    fruit_detector_node = Node(
        package='fruit_detector',
        executable='fruit_detector_node',
        name='fruit_detector',
        output='screen',
        parameters=[{
            'left_image_topic': '/left_cam/image_raw',
            'right_image_topic': '/right_cam/image_raw',
            'model_path_a': model_path_a,
            'model_path_b': model_path_b,
            'model_path_c': model_path_c,
            'conf_threshold': 0.5,
            'iou_threshold': 0.45
        }]
    )
    
    # 使用TimerAction延迟13秒启动水果识别（等待扫码先启动）
    delayed_fruit_detector = TimerAction(
        period=13.0,  # 延迟13秒，等待扫码先启动
        actions=[fruit_detector_node]
    )
    
    # 语音规划节点
    voice_planner_node = Node(
        package='voice_planner',
        executable='voice_planner_node',
        name='voice_planner',
        output='screen'
    )
    
    # 使用TimerAction延迟16秒启动语音规划（等待水果识别先启动）
    delayed_voice_planner = TimerAction(
        period=16.0,  # 延迟16秒，等待水果识别先启动
        actions=[voice_planner_node]
    )
    
    # 返回LaunchDescription
    return LaunchDescription([
        lidar_node,
        delayed_demo,  # 延迟2秒启动MoveIt demo（包含RViz和joint_state_publisher_gui）
        cartographer_localization,  # 延迟4秒启动定位
        delayed_arm_control,  # 延迟5秒启动arm_control相关节点（第一个启动）
        delayed_nav_node,  # 延迟6秒启动导航
        delayed_dual_camera,  # 延迟7秒启动双相机（第二个启动，等待arm_control）
        delayed_qr_recognition,  # 延迟10秒启动扫码节点（第三个启动，等待相机）
        delayed_fruit_detector,  # 延迟13秒启动水果识别节点（第四个启动，等待扫码）
        # delayed_voice_planner,  # 延迟16秒启动语音规划节点（第五个启动，等待水果识别）
    ])
